#include "bpcontrolhandle.h"
#include "log/zlog.hh"
#include "rtbus/rtbus.h"

static rtdev::BPControlHandle control_handle;  // 手柄设备

const uint8_t rtdev::BPControlHandle::ZUC_BP_SIGNAL_NONE_PRESD = 0x00;
const uint8_t rtdev::BPControlHandle::SYS_BP_SIGNAL_POWER_PRESD = 0x01;
const uint8_t rtdev::BPControlHandle::ZUC_BP_SIGNAL_POWER_ROBOT_PRESD = 0x2;
const uint8_t rtdev::BPControlHandle::ZUC_BP_SIGNAL_RETURN_PRESD = 0x04;
const uint8_t rtdev::BPControlHandle::ZUC_BP_SIGNAL_RUN_PAUSE_PRESD = 0x08;
const uint8_t rtdev::BPControlHandle::ZUC_BP_SIGNAL_TERMINATE_PRESD = 0x10;
const uint8_t rtdev::BPControlHandle::ZUC_BP_SIGNAL_LOCK_PRESD = 0x20;
const uint8_t rtdev::BPControlHandle::ZUC_BP_SIGNAL_EN_ROBOT_PRESD = 0x22;

const uint32_t rtdev::BPControlHandle::ZUC_BP_SHORT_PRESS_TIME = 15;

rtdev::BPControlHandle::BPControlHandle() : rtdev::RtDev(DEV_BP_HANDLE)
{
    memset(&handle_cmd_, 0, sizeof(ControlHanldeStatus));
    memset(&handle_status_, 0, sizeof(ControlHanldeStatus));
    memset(&button_event_status_, 0, sizeof(BPHandleButton));

    button_event_[ZUC_BP_SIGNAL_NONE_PRESD] = ButtonEvent{ZUC_BP_SHORT_PRESS_TIME, 0, 0, 0};
    button_event_[SYS_BP_SIGNAL_POWER_PRESD] = ButtonEvent{BP_CONTROL_FRQ * 3, 0, 0, 1};
    button_event_[ZUC_BP_SIGNAL_POWER_ROBOT_PRESD] = ButtonEvent{ZUC_BP_SHORT_PRESS_TIME, 0, 0, 0};
    button_event_[ZUC_BP_SIGNAL_RETURN_PRESD] = ButtonEvent{ZUC_BP_SHORT_PRESS_TIME, 0, 0, 0};
    button_event_[ZUC_BP_SIGNAL_RUN_PAUSE_PRESD] = ButtonEvent{ZUC_BP_SHORT_PRESS_TIME, 0, 0, 0};
    button_event_[ZUC_BP_SIGNAL_TERMINATE_PRESD] = ButtonEvent{ZUC_BP_SHORT_PRESS_TIME, 0, 0, 0};
    button_event_[ZUC_BP_SIGNAL_LOCK_PRESD] = ButtonEvent{BP_CONTROL_FRQ * 3, 0, 0, 1};
    button_event_[ZUC_BP_SIGNAL_EN_ROBOT_PRESD] = ButtonEvent{ZUC_BP_SHORT_PRESS_TIME, 0, 0, 0};

    // 默认参数
    handle_cmd_.bp_rgb_lock_color = ZUC_BP_RGB_YELLOW;
    handle_cmd_.bp_rgb_lock_brightness = 50;
    handle_cmd_.bp_rgb_logol_brightness = 50;
    handle_cmd_.heartbeat = 10;
}

rtdev::BPControlHandle::~BPControlHandle() {}

void rtdev::BPControlHandle::update()
{
    RtDev::update();

    // 打开关闭机器人电源
    button_event_status_.bp_power_robot_on_off = button_event_[SYS_BP_SIGNAL_POWER_PRESD].is_trigger;
    button_event_status_.bp_power_robot_on_off = button_event_[ZUC_BP_SIGNAL_POWER_ROBOT_PRESD].is_trigger;
    // 使能机器人
    button_event_status_.bp_en_robot_on_off = button_event_[ZUC_BP_SIGNAL_EN_ROBOT_PRESD].is_trigger;
    // 回原
    button_event_status_.bp_return_init_on_off = button_event_[ZUC_BP_SIGNAL_RETURN_PRESD].is_trigger;

    button_event_status_.bp_pause_resume_on_off = button_event_[ZUC_BP_SIGNAL_RUN_PAUSE_PRESD].is_trigger;
    button_event_status_.bp_run_stop_on_off = button_event_[ZUC_BP_SIGNAL_TERMINATE_PRESD].is_trigger;
    button_event_status_.bp_lock_on_off = button_event_[ZUC_BP_SIGNAL_LOCK_PRESD].is_trigger;
}

void rtdev::BPControlHandle::reset_control_button_status(int id)
{
    for (auto& it : button_event_)
    {
        if (it.first == id)
        {
            continue;
        }
        it.second.count = 0;
        if (!it.second.is_last)
        {
            it.second.is_trigger = false;
        }
    }
}

int rtdev::BPControlHandle::comm_read_button_status(uint8_t data)
{
    keep_alive();
    curr_button_state_ = data;
    if (data == ZUC_BP_SIGNAL_NONE_PRESD)
    {
        reset_control_button_status(-1);  // 全部清除
    }
    else
    {
        if (button_event_.count(data))
        {
            uint32_t count = button_event_[data].count;
            reset_control_button_status(data);  // 清除其他事件状态
            if (count > button_event_[data].trigger_count)
            {
                bool last_trigger = button_event_[data].is_trigger;
                if (button_event_[data].is_last)
                {
                    button_event_[data].is_trigger = !button_event_[data].is_trigger;  // 取反事件
                }
                else
                {
                    button_event_[data].is_trigger = true;  // 置位事件
                }
                if (last_trigger != button_event_[data].is_trigger)
                {
                    zlog()->info("[BP] button event {} is triggered to {}", data, button_event_[data].is_trigger);
                }
                button_event_[data].count = 0;
            }
            else
            {
                button_event_[data].count++;
            }
        }
    }
    update_bp_cmd();
    return 0;
}

BPHandleButton rtdev::BPControlHandle::handle_button_event_status() { return button_event_status_; }

void rtdev::BPControlHandle::beep_when_press()
{
    int half_second_count = BP_CONTROL_FRQ / 2;
    if (curr_button_state_ != ZUC_BP_SIGNAL_NONE_PRESD)
    {
        // 按键按下，间隔蜂鸣
        if (bp_beep_cnt_ < half_second_count)
        {
            bp_beep_cnt_++;
            if (bp_beep_cnt_ < half_second_count / 2)
            {
                handle_cmd_.bp_beep = 1;
            }
            else
            {
                handle_cmd_.bp_beep = 0;
            }
        }
        else
        {
            bp_beep_cnt_ = 0;
        }
    }
    else
    {
        handle_cmd_.bp_beep = 0;
    }
}

void rtdev::BPControlHandle::update_lock_light()
{ /**
	 * @brief 
	 * 	when bp is locked lock RGB is yellow
	 * 	otherwise RGB off when return is not pressed
	 */
    if (button_event_status_.bp_lock_on_off == 1)
    {
        /**
		 * @brief 
		 * 	lock RGB blue if the robot is already in init position
		 *  but the return button is still pressed
		 */
        if (button_event_status_.bp_return_init_on_off == 1)
        {
            if (rtrn_init_pos_done_)
            {
                handle_cmd_.bp_rgb_lock_color = ZUC_BP_RGB_BLUE;
                handle_cmd_.bp_rgb_lock_brightness = 98;
            }
            else
            {
                handle_cmd_.bp_rgb_lock_color = ZUC_BP_RGB_OFF;
                handle_cmd_.bp_rgb_lock_brightness = 0;
            }
        }
        else
        {
            handle_cmd_.bp_rgb_lock_color = ZUC_BP_RGB_OFF;
            handle_cmd_.bp_rgb_lock_brightness = 0;
        }
    }
    else
    {
        handle_cmd_.bp_rgb_lock_color = ZUC_BP_RGB_YELLOW;
        handle_cmd_.bp_rgb_lock_brightness = 60;
    }
}

void rtdev::BPControlHandle::update_bp_cmd()
{
    beep_when_press();
    update_lock_light();
}

rtdev::BPControlHandle* rtdev::get_bp_handle() { return &control_handle; }

int rtdev::BPControlHandle::boot_init()
{
    bus_id_ = -1;
    comm_jkcan_map();
    return 0;
}
void rtdev::BPControlHandle::comm_jkcan_map()
{
    //手柄状态 stat
    // rtbus::BusMgr::get()->register_rx_handle(0x191, [this](uint32_t busid, uint8_t* data, size_t size) -> int {
    //     if ((int)busid != get_master_id() && get_master_id() != -1)
    //     {
    //         comm_jkcan_tx_map(busid);
    //     }
    //     comm_read_beep_status(data[0]);
    //     comm_read_button_status(data[1]);
    //     comm_read_lock_color(data[2]);
    //     comm_read_lock_brightness(data[3]);
    //     comm_read_logo_color(data[4]);
    //     comm_read_logo_brightness(data[5]);
    //     comm_read_heartbeat(data[6]);
    //     return 0;
    // });
}
void rtdev::BPControlHandle::comm_jkcan_tx_map(int busid)
{
    // rtbus::BusMgr::get()->register_tx_handle(busid, 0x211, [](uint32_t id, uint8_t* data, size_t size) -> int {
    //     //bp cmd 手柄控制指令
    //     rtdev::BPControlHandle* bp = rtdev::get_bp_handle();
    //     if (!bp)
    //     {
    //         return -1;
    //     }
    //     bp->comm_write_beep_status(&data[0]);
    //     bp->comm_write_lock_color(&data[1]);
    //     bp->comm_write_lock_brightness(&data[2]);
    //     bp->comm_write_logo_color(&data[3]);
    //     bp->comm_write_logo_brightness(&data[4]);
    //     bp->comm_write_heartbeat(&data[5]);
    //     data[6] = 0;  // 预留
    //     data[7] = 0;  // 预留
    //     return 8;
    // });
    // set_master(busid);
}
